/* GJJoy.cc -- Módulo de controle do joystick
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part of GEARJoystick.
 * 
 * GEARJoystick is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARJoystick is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <iostream>

#include <math.h>
#include <fcntl.h>
#include <unistd.h>

#include "GJJoy.hh"

using namespace std;

GJJoy::GJJoy()
{
    /* Zerando ponteiros */

    /* Zerando variáveis */
    joystick_fd_ = 0;
    vx_ = 0;
    vy_ = 0;
    va_ = 0;
    dribble_ = 0;
    comtype_ = NONE;
    
}

GJJoy::~GJJoy()
{
    close( joystick_fd_ );
}

void
GJJoy::connectToMedula()
{
    robot_ = new GMRobot( robot_id_,
			  robot_end_,
			  server_addr_,
			  server_port_ );
}

void
GJJoy::Init( CommLineArgs line_args )
{
    // Iniciando variáveis
    joystick_path_ = line_args.getJoystickPath();
    server_port_ = line_args.getServerPort();
    server_addr_ = line_args.getServerAddr();
    robot_id_ = line_args.getRobotID();
    robot_end_ = line_args.getRobotEnd();
    
    
    joystick_fd_ = open( joystick_path_.c_str(), O_RDONLY | O_NONBLOCK);

    if (joystick_fd_ < 0)
	cerr << "Caminho errado para o joystick" << endl;

    connectToMedula();
    
}

int
GJJoy::readJoystickEvent()
{
    int bytes;

    bytes = read(joystick_fd_, &(jse_), sizeof(js_event)); 

    if (bytes == -1)
	return 0;
    
    if (bytes == sizeof(js_event))
	return 1;
	
    cerr << "Quantidade inesperada de bytes";

    return -1;
}

void
GJJoy::processEvent()
{
    // Essa conta bizarra é o seguinte: O máximo valor que o controle
    // passa é 32767 assim, divido por dois para que, na divisão
    // abaixo, o valor seja 2, que significará 2 m/s
    float limite = 32767.f/2.f;
    float limite_rot = ((2*M_PI)/(32767.f/2.f));

    switch( jse_.type )
    {
    case JS_EVENT_AXIS:
    {        
        switch (jse_.number )
        {
        case 4:  /* Direcional X */
        {
            vx_ = jse_.value/limite;
            comtype_ = ROBOT_SET_VEL;
            break;
        }
        case 0: /* Analalógico X */
        {
            vx_ = jse_.value/limite;
            comtype_ = ROBOT_SET_VEL;
            break;
        }
        case 5: /* Direcional Y */
        {
            vy_ = -1 * (jse_.value/limite);
            comtype_ = ROBOT_SET_VEL;
            break;
        }
        case 1: /* Analógico Y */
        {
            vy_ = -1 * (jse_.value/limite);
            comtype_ = ROBOT_SET_VEL;
            break;
        }
        case 2: /* Analógico da direita */
        {
            va_ = jse_.value * limite_rot;
            comtype_ = ROBOT_SET_VEL;
            break;
        }
        }
        break;
    }
    case JS_EVENT_BUTTON:
    {
        switch( jse_.number )
        {
        case 6: /* L1 = Liga o rolinho */
        {
			dribble();
            break;
        }
        case 7: /* R1 = Liga rolinho */
		{
			dribble();
			break;
		}
        case 5: /* L1 = Liga rolinho */
		{
			dribble();
			break;
		}
        default:
        {
			/* desliga o rolinho se ele estiver ligado */
			if (dribble_ == 1) {
				dribble_ = 0;
            	comtype_ = ROBOT_DRIBBLE;
			}
			else {			
	            comtype_ = NONE;
			}
            break;
        }
        }
        break;
    }
    }
}


void
GJJoy::sendCommandToMedula()
{
    switch (comtype_)
    {
    case ROBOT_SET_VEL:
	robot_->SetVel( vx_, vy_, va_ );
	break;
    case ROBOT_KICK_STRONG:
	robot_->Kick( 0 );
	break;
    case ROBOT_KICK_WEAK:
	robot_->Kick( 1 );
	break;
    case ROBOT_DRIBBLE:
	robot_->DribbleEnable( dribble_ );
	break;
    case NONE:
	break;
    }
}

void
GJJoy::dribble()
{
    if ( dribble_ == 0 ) {
        dribble_ = 1;
        comtype_ = ROBOT_DRIBBLE;
    }
    else {
        comtype_ = NONE;
    }
}
